//  mmath.c - matrix math (loops unrolled for speed)
//  ------------------------------------------------

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mmath.h"

//  allocate memory for 1D double array (memset to zeros)
double* dvecalloc(char n) {
	char x;
	double *v = (double*)malloc(sizeof(double) * n);
	if (v == NULL) return NULL;
	for (x = 0; x < n; x++) {
		v[x] = 0;
	}
	return v;
}

// free a vector that was allocated with dvecalloc and set pointer to NULL
void dvecfree(double* v) {
	free(v);
	v = NULL;
}

// allocate memory for 2D double array (memset to zeros);
double** dmatalloc(char nx, char ny) {
	double **m;
	char x, y;

	m = (double**)malloc(sizeof(double*) * nx);
	if (m != NULL) {
		for (x = 0; x < nx; x++) {
			m[x] = (double*)malloc(sizeof(double) * ny);
			if (m[x] == NULL) {
				return NULL;
			}
			for (y = 0; y < ny; y++) {
				m[x][y] = 0;
			}
		}
		return m;
	}
	else {
        return NULL;
    }
 }
 
 // free a matrix that was allocated with dmatalloc and set pointers to NULL
 void dmatfree(double** m, char nx) {
	char x;
	
	for (x = 0; x < nx; x++) {
		free(m[x]);
		m[x] = NULL;
	}
	free(m);
	m = NULL;
 }
 
 // copy one 3x1 vector into another (modifies dst)
 void _vcopy3(double* src, double* dst) {
 	dst[0] = src[0];
 	dst[1] = src[1];
 	dst[2] = src[2];
 }
 
 // copy one 4x1 vector into another (modifies dst)
 void _vcopy4(double* src, double* dst) {
 	dst[0] = src[0];
 	dst[1] = src[1];
 	dst[2] = src[2];
 	dst[3] = src[3];
 }

// transpose the 3x3 matrix m (allocates new matrix)
double** T3(double** m) {
	double** mT = dmatalloc(3, 3);
	mT[0][0] = m[0][0];
	mT[0][1] = m[1][0];
	mT[0][2] = m[2][0];
	mT[1][0] = m[0][1];
	mT[1][1] = m[1][1];
	mT[1][2] = m[2][1];
	mT[2][0] = m[0][2];
	mT[2][1] = m[1][2];
	mT[2][2] = m[2][2];
	return mT;
}

// multiply two 3x3 matrices (allocates new matrix)
double** mmult3(double** m1, double** m2) {
	double** m = dmatalloc(3, 3);
	_mmult3(m, m1, m2);
	return m;
}

// multiply two 3x3 matrices (modifies existing matrix)
void _mmult3(double** m, double** m1, double** m2) {
	m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
	m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
	m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
	m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0];
	m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
	m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2];
	m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0];
	m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1];
	m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2];
}

// multiply two 4x4 matrices (allocates new matrix)
double** mmult4(double** m1, double** m2) {
	double** m = dmatalloc(4, 4);
	_mmult4(m, m1, m2);
	return m;
}

// multiply two 4x4 matrices (modifies existing data)
void _mmult4(double** m, double** m1, double** m2) {
	m[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0] + m1[0][3]*m2[3][0];
	m[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1] + m1[0][3]*m2[3][1];
	m[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2] + m1[0][3]*m2[3][2];
	m[0][3] = m1[0][0]*m2[0][3] + m1[0][1]*m2[1][3] + m1[0][2]*m2[2][3] + m1[0][3]*m2[3][3];
	m[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] + m1[1][2]*m2[2][0] + m1[1][3]*m2[3][0];
	m[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1] + m1[1][3]*m2[3][1];
	m[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] + m1[1][2]*m2[2][2] + m1[1][3]*m2[3][2];
	m[1][3] = m1[1][0]*m2[0][3] + m1[1][1]*m2[1][3] + m1[1][2]*m2[2][3] + m1[1][3]*m2[3][3];
	m[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] + m1[2][2]*m2[2][0] + m1[2][3]*m2[3][0];
	m[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] + m1[2][2]*m2[2][1] + m1[2][3]*m2[3][1];
	m[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] + m1[2][2]*m2[2][2] + m1[2][3]*m2[3][2];
	m[2][3] = m1[2][0]*m2[0][3] + m1[2][1]*m2[1][3] + m1[2][2]*m2[2][3] + m1[2][3]*m2[3][3];
	m[3][0] = m1[3][0]*m2[0][0] + m1[3][1]*m2[1][0] + m1[3][2]*m2[2][0] + m1[3][3]*m2[3][0];
	m[3][1] = m1[3][0]*m2[0][1] + m1[3][1]*m2[1][1] + m1[3][2]*m2[2][1] + m1[3][3]*m2[3][1];
	m[3][2] = m1[3][0]*m2[0][2] + m1[3][1]*m2[1][2] + m1[3][2]*m2[2][2] + m1[3][3]*m2[3][2];
	m[3][3] = m1[3][0]*m2[0][3] + m1[3][1]*m2[1][3] + m1[3][2]*m2[2][3] + m1[3][3]*m2[3][3];
}

// multiply a 3x3 matrix by a 3x1 vector (allocates new vector)
double* mvmult3(double** m, double* v) {
	double* p = dvecalloc(3);
	_mvmult3(p, m, v);
	return p;
}

// multiply a 3x3 matrix by a 3x1 vector (modifies existing vector)
void _mvmult3(double* p, double** m, double* v) {
	p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
	p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
	p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}

// multiply a 4x4 matrix by a 4x1 vector (allocates new vector)
double* mvmult4(double** m, double* v) {
	double* p = dvecalloc(4);
	_mvmult4(p, m, v);
	return p;
}

// multiply a 4x4 matrix by a 4x1 vector (modifies existing vector)
void _mvmult4(double* p, double** m, double* v) {
	p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2] + m[0][3]*v[3];
	p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2] + m[1][3]*v[3];
	p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2] + m[2][3]*v[3];
	p[3] = m[3][0]*v[0] + m[3][1]*v[1] + m[3][2]*v[2] + m[3][3]*v[3];
}

// add two 3x1 vectors (allocates new vector)
double* vadd3(double* v1, double* v2) {
	double* v = dvecalloc(3);
	_vadd3(v, v1, v2);
	return v;
}

void _vadd3(double* v, double* v1, double* v2) {
	v[0] = v1[0] + v2[0];
	v[1] = v1[1] + v2[1];
	v[2] = v1[2] + v2[2];
}

// add two 4x1 vectors (allocates new vector)
double* vadd4(double* v1, double* v2) {
	double* v = dvecalloc(4);
	_vadd4(v, v1, v2);
	return v;
}

void _vadd4(double* v, double* v1, double* v2) {
	v[0] = v1[0] + v2[0];
	v[1] = v1[1] + v2[1];
	v[2] = v1[2] + v2[2];
	v[3] = v1[3] + v2[3];
}

// scale a 3x1 vector (modifies existing vector)
void _vscale3(double* v, double s) {
	v[0] *= s;
	v[1] *= s;
	v[2] *= s;
}

// scale a 4x1 vector (modifies existing vector)
void _vscale4(double* v, double s) {
	v[0] *= s;
	v[1] *= s;
	v[2] *= s;
	v[3] *= s;
}

void _normalize4(double* v) {
	double mag = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
	v[0] /= mag;
	v[1] /= mag;
	v[2] /= mag;
	v[3] /= mag;
}

void vprint(double *v, char n) {
	char i;
	printf("[ ");
	for (i = 0; i < n; i++) {
		printf("%lf ", v[i]);
	}
	printf("]\n");
}

void mprint(double** m, char nx, char ny) {
	char x, y;
	
	for (x = 0; x < nx; x++) {
		printf("\t");
		if (x == 0) printf("[ ");
		else printf("  ");
		for (y = 0; y < ny; y++) {
			printf("%lf ", m[x][y]);
		}
		if (x == nx-1) printf("]\n");
		else printf("\n");
	}
}
